#include "clib_hash_md5.h"



#define CLIB_HASH_MD5_F(x, y, z)   (((x) & (y)) | ((~(x)) & (z)))
#define CLIB_HASH_MD5_G(x, y, z)   (((x) & (z)) | ((y) & (~(z))))
#define CLIB_HASH_MD5_H(x, y, z)   ((x) ^ (y) ^ (z))
#define CLIB_HASH_MD5_I(x, y, z)   ((y) ^ ((x) | (~(z))))

#define TCLIB_HASH_MD5_ROTATE_LEFT(x, n)    (((x) << (n)) | ((x) >> (32 - (n))))

#define CLIB_HASH_MD5_FF(a, b, c, d, x, s, ac) {(a) += CLIB_HASH_MD5_F ((b), (c), (d)) + (x) + (clib_uint32_t)(ac); (a) = TCLIB_HASH_MD5_ROTATE_LEFT ((a), (s)); (a) += (b); }
#define CLIB_HASH_MD5_GG(a, b, c, d, x, s, ac) {(a) += CLIB_HASH_MD5_G ((b), (c), (d)) + (x) + (clib_uint32_t)(ac); (a) = TCLIB_HASH_MD5_ROTATE_LEFT ((a), (s)); (a) += (b); }
#define CLIB_HASH_MD5_HH(a, b, c, d, x, s, ac) {(a) += CLIB_HASH_MD5_H ((b), (c), (d)) + (x) + (clib_uint32_t)(ac); (a) = TCLIB_HASH_MD5_ROTATE_LEFT ((a), (s)); (a) += (b); }
#define CLIB_HASH_MD5_II(a, b, c, d, x, s, ac) {(a) += CLIB_HASH_MD5_I ((b), (c), (d)) + (x) + (clib_uint32_t)(ac); (a) = TCLIB_HASH_MD5_ROTATE_LEFT ((a), (s)); (a) += (b); }

// Constants for transformation
#define CLIB_HASH_MD5_S11 7  // Round 1
#define CLIB_HASH_MD5_S12 12
#define CLIB_HASH_MD5_S13 17
#define CLIB_HASH_MD5_S14 22
#define CLIB_HASH_MD5_S21 5  // Round 2
#define CLIB_HASH_MD5_S22 9
#define CLIB_HASH_MD5_S23 14
#define CLIB_HASH_MD5_S24 20
#define CLIB_HASH_MD5_S31 4  // Round 3
#define CLIB_HASH_MD5_S32 11
#define CLIB_HASH_MD5_S33 16
#define CLIB_HASH_MD5_S34 23
#define CLIB_HASH_MD5_S41 6  // Round 4
#define CLIB_HASH_MD5_S42 10
#define CLIB_HASH_MD5_S43 15
#define CLIB_HASH_MD5_S44 21


static clib_uint8_t g_md5_padding[64] =
        {
                0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
        };


static void clib_hash_md5_transform(clib_uint32_t *sp, const clib_uint32_t *ip) {
    clib_check_if_true_return(sp == NULL);
    clib_check_if_true_return(ip == NULL);
    clib_uint32_t a = sp[0], b = sp[1], c = sp[2], d = sp[3];

    // round 1
    CLIB_HASH_MD5_FF(a, b, c, d, ip[0], CLIB_HASH_MD5_S11, (clib_uint32_t) 3614090360u); /* 1 */
    CLIB_HASH_MD5_FF(d, a, b, c, ip[1], CLIB_HASH_MD5_S12, (clib_uint32_t) 3905402710u); /* 2 */
    CLIB_HASH_MD5_FF(c, d, a, b, ip[2], CLIB_HASH_MD5_S13, (clib_uint32_t) 606105819u); /* 3 */
    CLIB_HASH_MD5_FF(b, c, d, a, ip[3], CLIB_HASH_MD5_S14, (clib_uint32_t) 3250441966u); /* 4 */
    CLIB_HASH_MD5_FF(a, b, c, d, ip[4], CLIB_HASH_MD5_S11, (clib_uint32_t) 4118548399u); /* 5 */
    CLIB_HASH_MD5_FF(d, a, b, c, ip[5], CLIB_HASH_MD5_S12, (clib_uint32_t) 1200080426u); /* 6 */
    CLIB_HASH_MD5_FF(c, d, a, b, ip[6], CLIB_HASH_MD5_S13, (clib_uint32_t) 2821735955u); /* 7 */
    CLIB_HASH_MD5_FF(b, c, d, a, ip[7], CLIB_HASH_MD5_S14, (clib_uint32_t) 4249261313u); /* 8 */
    CLIB_HASH_MD5_FF(a, b, c, d, ip[8], CLIB_HASH_MD5_S11, (clib_uint32_t) 1770035416u); /* 9 */
    CLIB_HASH_MD5_FF(d, a, b, c, ip[9], CLIB_HASH_MD5_S12, (clib_uint32_t) 2336552879u); /* 10 */
    CLIB_HASH_MD5_FF(c, d, a, b, ip[10], CLIB_HASH_MD5_S13, (clib_uint32_t) 4294925233u); /* 11 */
    CLIB_HASH_MD5_FF(b, c, d, a, ip[11], CLIB_HASH_MD5_S14, (clib_uint32_t) 2304563134u); /* 12 */
    CLIB_HASH_MD5_FF(a, b, c, d, ip[12], CLIB_HASH_MD5_S11, (clib_uint32_t) 1804603682u); /* 13 */
    CLIB_HASH_MD5_FF(d, a, b, c, ip[13], CLIB_HASH_MD5_S12, (clib_uint32_t) 4254626195u); /* 14 */
    CLIB_HASH_MD5_FF(c, d, a, b, ip[14], CLIB_HASH_MD5_S13, (clib_uint32_t) 2792965006u); /* 15 */
    CLIB_HASH_MD5_FF(b, c, d, a, ip[15], CLIB_HASH_MD5_S14, (clib_uint32_t) 1236535329u); /* 16 */

    // round 2
    CLIB_HASH_MD5_GG(a, b, c, d, ip[1], CLIB_HASH_MD5_S21, (clib_uint32_t) 4129170786u); /* 17 */
    CLIB_HASH_MD5_GG(d, a, b, c, ip[6], CLIB_HASH_MD5_S22, (clib_uint32_t) 3225465664u); /* 18 */
    CLIB_HASH_MD5_GG(c, d, a, b, ip[11], CLIB_HASH_MD5_S23, (clib_uint32_t) 643717713u); /* 19 */
    CLIB_HASH_MD5_GG(b, c, d, a, ip[0], CLIB_HASH_MD5_S24, (clib_uint32_t) 3921069994u); /* 20 */
    CLIB_HASH_MD5_GG(a, b, c, d, ip[5], CLIB_HASH_MD5_S21, (clib_uint32_t) 3593408605u); /* 21 */
    CLIB_HASH_MD5_GG(d, a, b, c, ip[10], CLIB_HASH_MD5_S22, (clib_uint32_t) 38016083u); /* 22 */
    CLIB_HASH_MD5_GG(c, d, a, b, ip[15], CLIB_HASH_MD5_S23, (clib_uint32_t) 3634488961u); /* 23 */
    CLIB_HASH_MD5_GG(b, c, d, a, ip[4], CLIB_HASH_MD5_S24, (clib_uint32_t) 3889429448u); /* 24 */
    CLIB_HASH_MD5_GG(a, b, c, d, ip[9], CLIB_HASH_MD5_S21, (clib_uint32_t) 568446438u); /* 25 */
    CLIB_HASH_MD5_GG(d, a, b, c, ip[14], CLIB_HASH_MD5_S22, (clib_uint32_t) 3275163606u); /* 26 */
    CLIB_HASH_MD5_GG(c, d, a, b, ip[3], CLIB_HASH_MD5_S23, (clib_uint32_t) 4107603335u); /* 27 */
    CLIB_HASH_MD5_GG(b, c, d, a, ip[8], CLIB_HASH_MD5_S24, (clib_uint32_t) 1163531501u); /* 28 */
    CLIB_HASH_MD5_GG(a, b, c, d, ip[13], CLIB_HASH_MD5_S21, (clib_uint32_t) 2850285829u); /* 29 */
    CLIB_HASH_MD5_GG(d, a, b, c, ip[2], CLIB_HASH_MD5_S22, (clib_uint32_t) 4243563512u); /* 30 */
    CLIB_HASH_MD5_GG(c, d, a, b, ip[7], CLIB_HASH_MD5_S23, (clib_uint32_t) 1735328473u); /* 31 */
    CLIB_HASH_MD5_GG(b, c, d, a, ip[12], CLIB_HASH_MD5_S24, (clib_uint32_t) 2368359562u); /* 32 */

    // round 3
    CLIB_HASH_MD5_HH(a, b, c, d, ip[5], CLIB_HASH_MD5_S31, (clib_uint32_t) 4294588738u); /* 33 */
    CLIB_HASH_MD5_HH(d, a, b, c, ip[8], CLIB_HASH_MD5_S32, (clib_uint32_t) 2272392833u); /* 34 */
    CLIB_HASH_MD5_HH(c, d, a, b, ip[11], CLIB_HASH_MD5_S33, (clib_uint32_t) 1839030562u); /* 35 */
    CLIB_HASH_MD5_HH(b, c, d, a, ip[14], CLIB_HASH_MD5_S34, (clib_uint32_t) 4259657740u); /* 36 */
    CLIB_HASH_MD5_HH(a, b, c, d, ip[1], CLIB_HASH_MD5_S31, (clib_uint32_t) 2763975236u); /* 37 */
    CLIB_HASH_MD5_HH(d, a, b, c, ip[4], CLIB_HASH_MD5_S32, (clib_uint32_t) 1272893353u); /* 38 */
    CLIB_HASH_MD5_HH(c, d, a, b, ip[7], CLIB_HASH_MD5_S33, (clib_uint32_t) 4139469664u); /* 39 */
    CLIB_HASH_MD5_HH(b, c, d, a, ip[10], CLIB_HASH_MD5_S34, (clib_uint32_t) 3200236656u); /* 40 */
    CLIB_HASH_MD5_HH(a, b, c, d, ip[13], CLIB_HASH_MD5_S31, (clib_uint32_t) 681279174u); /* 41 */
    CLIB_HASH_MD5_HH(d, a, b, c, ip[0], CLIB_HASH_MD5_S32, (clib_uint32_t) 3936430074u); /* 42 */
    CLIB_HASH_MD5_HH(c, d, a, b, ip[3], CLIB_HASH_MD5_S33, (clib_uint32_t) 3572445317u); /* 43 */
    CLIB_HASH_MD5_HH(b, c, d, a, ip[6], CLIB_HASH_MD5_S34, (clib_uint32_t) 76029189u); /* 44 */
    CLIB_HASH_MD5_HH(a, b, c, d, ip[9], CLIB_HASH_MD5_S31, (clib_uint32_t) 3654602809u); /* 45 */
    CLIB_HASH_MD5_HH(d, a, b, c, ip[12], CLIB_HASH_MD5_S32, (clib_uint32_t) 3873151461u); /* 46 */
    CLIB_HASH_MD5_HH(c, d, a, b, ip[15], CLIB_HASH_MD5_S33, (clib_uint32_t) 530742520u); /* 47 */
    CLIB_HASH_MD5_HH(b, c, d, a, ip[2], CLIB_HASH_MD5_S34, (clib_uint32_t) 3299628645u); /* 48 */

    // round 4
    CLIB_HASH_MD5_II(a, b, c, d, ip[0], CLIB_HASH_MD5_S41, (clib_uint32_t) 4096336452u); /* 49 */
    CLIB_HASH_MD5_II(d, a, b, c, ip[7], CLIB_HASH_MD5_S42, (clib_uint32_t) 1126891415u); /* 50 */
    CLIB_HASH_MD5_II(c, d, a, b, ip[14], CLIB_HASH_MD5_S43, (clib_uint32_t) 2878612391u); /* 51 */
    CLIB_HASH_MD5_II(b, c, d, a, ip[5], CLIB_HASH_MD5_S44, (clib_uint32_t) 4237533241u); /* 52 */
    CLIB_HASH_MD5_II(a, b, c, d, ip[12], CLIB_HASH_MD5_S41, (clib_uint32_t) 1700485571u); /* 53 */
    CLIB_HASH_MD5_II(d, a, b, c, ip[3], CLIB_HASH_MD5_S42, (clib_uint32_t) 2399980690u); /* 54 */
    CLIB_HASH_MD5_II(c, d, a, b, ip[10], CLIB_HASH_MD5_S43, (clib_uint32_t) 4293915773u); /* 55 */
    CLIB_HASH_MD5_II(b, c, d, a, ip[1], CLIB_HASH_MD5_S44, (clib_uint32_t) 2240044497u); /* 56 */
    CLIB_HASH_MD5_II(a, b, c, d, ip[8], CLIB_HASH_MD5_S41, (clib_uint32_t) 1873313359u); /* 57 */
    CLIB_HASH_MD5_II(d, a, b, c, ip[15], CLIB_HASH_MD5_S42, (clib_uint32_t) 4264355552u); /* 58 */
    CLIB_HASH_MD5_II(c, d, a, b, ip[6], CLIB_HASH_MD5_S43, (clib_uint32_t) 2734768916u); /* 59 */
    CLIB_HASH_MD5_II(b, c, d, a, ip[13], CLIB_HASH_MD5_S44, (clib_uint32_t) 1309151649u); /* 60 */
    CLIB_HASH_MD5_II(a, b, c, d, ip[4], CLIB_HASH_MD5_S41, (clib_uint32_t) 4149444226u); /* 61 */
    CLIB_HASH_MD5_II(d, a, b, c, ip[11], CLIB_HASH_MD5_S42, (clib_uint32_t) 3174756917u); /* 62 */
    CLIB_HASH_MD5_II(c, d, a, b, ip[2], CLIB_HASH_MD5_S43, (clib_uint32_t) 718787259u); /* 63 */
    CLIB_HASH_MD5_II(b, c, d, a, ip[9], CLIB_HASH_MD5_S44, (clib_uint32_t) 3951481745u); /* 64 */

    sp[0] += a;
    sp[1] += b;
    sp[2] += c;
    sp[3] += d;
}

int clib_hash_md5_simple(clib_uint8_t const *in_data, size_t in_size, clib_uint8_t *out_data) {
    struct clib_hash_md5 md5;
    if (CLIB_RES_OK != clib_hash_md5_init(&md5)) {
        return CLIB_RES_OTHER_ERROR;
    }
    if (CLIB_RES_OK != clib_hash_md5_update(&md5, in_data, in_size)) {
        return CLIB_RES_OTHER_ERROR;
    }
    if (CLIB_RES_OK != clib_hash_md5_final(&md5, out_data)) {
        return CLIB_RES_OTHER_ERROR;
    }
    return CLIB_RES_OK;
}

int clib_hash_md5_init(struct clib_hash_md5 *md5) {
    clib_check_if_true_return_value(md5 == NULL, CLIB_RES_PARAM_ERROR);
    md5->i[0] = md5->i[1] = (clib_uint32_t) 0;
    md5->sp[0] = (clib_uint32_t) 0x67452301 + (0 * 11);
    md5->sp[1] = (clib_uint32_t) 0xefcdab89 + (0 * 71);
    md5->sp[2] = (clib_uint32_t) 0x98badcfe + (0 * 37);
    md5->sp[3] = (clib_uint32_t) 0x10325476 + (0 * 97);
    return CLIB_RES_OK;
}


int clib_hash_md5_update(struct clib_hash_md5 *md5, clib_uint8_t const *data, size_t size) {
    clib_check_if_true_return_value(md5 == NULL, CLIB_RES_PARAM_ERROR);
    clib_check_if_true_return_value(data == NULL, CLIB_RES_PARAM_ERROR);

    clib_uint32_t ip[16];
    size_t i = 0, ii = 0;
    clib_int32_t mdi = (clib_int32_t) ((md5->i[0] >> 3) & 0x3F);
    if ((md5->i[0] + ((clib_uint32_t) size << 3)) < md5->i[0]) {
        md5->i[1]++;
    }
    md5->i[0] += ((clib_uint32_t) size << 3);
    md5->i[1] += ((clib_uint32_t) size >> 29);
    while (size--) {
        md5->ip[mdi++] = *data++;
        if (mdi == 0x40) {
            for (i = 0, ii = 0; i < 16; i++, ii += 4) {
                ip[i] = (((clib_uint32_t) md5->ip[ii + 3]) << 24)
                        | (((clib_uint32_t) md5->ip[ii + 2]) << 16)
                        | (((clib_uint32_t) md5->ip[ii + 1]) << 8)
                        | ((clib_uint32_t) md5->ip[ii]);
            }
            clib_hash_md5_transform(md5->sp, ip);
            mdi = 0;
        }
    }
    return CLIB_RES_OK;
}

int clib_hash_md5_final(struct clib_hash_md5 *md5, clib_uint8_t *data) {
    clib_check_if_true_return_value(md5 == NULL, CLIB_RES_PARAM_ERROR);
    clib_check_if_true_return_value(data == NULL, CLIB_RES_PARAM_ERROR);

    clib_uint32_t ip[16];
    clib_int32_t mdi = 0;
    size_t i = 0;
    size_t ii = 0;
    size_t pad_n = 0;

    ip[14] = md5->i[0];
    ip[15] = md5->i[1];

    mdi = (clib_int32_t) ((md5->i[0] >> 3) & 0x3F);

    pad_n = (mdi < 56) ? (56 - mdi) : (120 - mdi);
    clib_hash_md5_update(md5, g_md5_padding, pad_n);

    for (i = 0, ii = 0; i < 14; i++, ii += 4) {
        ip[i] = (((clib_uint32_t) md5->ip[ii + 3]) << 24)
                | (((clib_uint32_t) md5->ip[ii + 2]) << 16)
                | (((clib_uint32_t) md5->ip[ii + 1]) << 8)
                | ((clib_uint32_t) md5->ip[ii]);
    }
    clib_hash_md5_transform(md5->sp, ip);

    for (i = 0, ii = 0; i < 4; i++, ii += 4) {
        md5->data[ii] = (clib_uint8_t) (md5->sp[i] & 0xff);
        md5->data[ii + 1] = (clib_uint8_t) ((md5->sp[i] >> 8) & 0xff);
        md5->data[ii + 2] = (clib_uint8_t) ((md5->sp[i] >> 16) & 0xff);
        md5->data[ii + 3] = (clib_uint8_t) ((md5->sp[i] >> 24) & 0xff);
    }

    memcpy(data, md5->data, 16);
    return CLIB_RES_OK;
}